#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace inv_pen
{
    class SituCmdPub
    {
    public:
        SituCmdPub(std::string, double, double);
        void FeedBackSubCallBack(const std_msgs::Float64 &msg);
        void loop(void);

    private:
        double gain;
        std_msgs::Float64 situ_cmd;
        std_msgs::Float64 u_feedback_msg;
        ros::NodeHandle pnh;
        ros::Subscriber u_feedback_sub;
        ros::Publisher situ_cmd_pub;
    };
}

inv_pen::SituCmdPub::SituCmdPub(std::string ns, double _gain, double _situ_cmd)
{
    pnh = ros::NodeHandle(ns);
    gain = _gain;
    situ_cmd.data = _situ_cmd;
    u_feedback_msg.data = 0.0;
    situ_cmd_pub = pnh.advertise<std_msgs::Float64>("/situ_cmd", 2);
    u_feedback_sub = pnh.subscribe("/unsigned_feedback", 2, &inv_pen::SituCmdPub::FeedBackSubCallBack, this);
}

void inv_pen::SituCmdPub::FeedBackSubCallBack(const std_msgs::Float64 &msg)
{
    u_feedback_msg.data = msg.data;
    //ROS_INFO("recieve situ_msg: %f",msg.data);
}

void inv_pen::SituCmdPub::loop(void)
{
    double tmp;
    std_msgs::Float64 error;
    ros::spinOnce();
    ros::param::get("/situ_cmd", situ_cmd.data);
    error.data = gain*situ_cmd.data - u_feedback_msg.data;
    situ_cmd_pub.publish(error);
    //ROS_INFO("pub error: %f",error.data);   
}

int main(int argc, char **argv)
{
    /*
    get situ_cmd from rosparam,and pub the error
    */
    ros::init(argc, argv, "situ_error_cmd_pub_node");
    double gain, situ_cmd;
    ros::param::get("/gain", gain);
    ros::param::get("/situ_cmd", gain);
    inv_pen::SituCmdPub situpub("", gain, situ_cmd);

    ros::Rate rate(20);
    while (ros::ok())
    {
        situpub.loop();
        rate.sleep();
    }
    ros::shutdown();
    return 0;
}